
// Written by michael oborne

#include <FastSerial.h>
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <SD.h>
#include <AP_Common.h>
#include <AP_GPS.h>         // ArduPilot GPS library
#include <GCS_MAVLink.h>    // MAVLink GCS definitions
#include <stdio.h> 
#include <stdlib.h> 
#include <math.h> 
#include "sha1.h"
#include "base64_enc.h"
#include "define.h"
#include "Parameters.h"
#include "GCS.h"

// SD must use dos filenames 8.3 no spaces
//

FastSerialPort0(Serial);        // u32/console
FastSerialPort1(Serial1);       // xbee
FastSerialPort2(Serial2);       // wifly
FastSerialPort3(Serial3);       // gps

static Parameters      g;

GCS_MAVLINK	xbee(Parameters::k_param_streamrates_port0);

int inByte = 0; 
int requeststep = 0;
bool websocketrunning = false;
bool socketopen = false;

float roll=0,pitch=0,yaw = 0;
static unsigned long 	fast_loopTimer;				// Time in miliseconds of main control loop

File myFile;

void setup()
{
  roll=0,pitch=0,yaw = 0;

  
  Serial.begin(115200, 128, 128);
  Serial2.begin(9600);
  Serial1.begin(57600);
  Serial3.begin(38400);
  
      Serial.println("Init mavlink");
    	// init the GCS
        xbee.init(&Serial1);
    
  // sd lib
  Serial.print("Initializing SD card...");
  // On the Ethernet Shield, CS is pin 4. It's set as an output by default.
  // Note that even if it's not used as the CS pin, the hardware SS pin 
  // (10 on most Arduino boards, 53 on the Mega) must be left as an output 
  // or the SD library functions will not work. 
   pinMode(53, OUTPUT);
   
  if (!SD.begin(53)) {
    Serial.println("initialization failed!");
    return;
  }
  Serial.println("initialization done.");
  
  
  Serial2.write("$$$");
readLineSerial2(400); 
  Serial2.write("set uart baud 115200\r");
readLineSerial2(400); 
readLineSerial2(400); 
   Serial2.write("\rset ip dhcp 0\r");
readLineSerial2(400);
readLineSerial2(400); 
  Serial2.write("set ip address 169.254.1.1\r");
readLineSerial2(400);
readLineSerial2(400); 
    Serial2.write("set ip netmask 255.255.0.0\r");
readLineSerial2(400);
readLineSerial2(400); 
  Serial2.write("set wlan join 4\r");
readLineSerial2(400);
readLineSerial2(400); 
 Serial2.write("set wlan ssid wifly\r");
readLineSerial2(400);
readLineSerial2(400); 
  Serial2.write("set wlan chan 1\r");
readLineSerial2(400);
readLineSerial2(400); 
 Serial2.write("set ip localport 80\r");
readLineSerial2(400);
readLineSerial2(400); 
 Serial2.write("set ip remote 80\r");
readLineSerial2(400);
readLineSerial2(400); 
  Serial2.write("set ip protocol 2\r");
readLineSerial2(400);
readLineSerial2(400);  
  Serial2.write("set comm idle 3\r");
readLineSerial2(2000);
readLineSerial2(2000); 
  Serial2.write("set comm remote 0\r");
readLineSerial2(400);
readLineSerial2(400); 
    Serial2.write("save\r");
readLineSerial2(400); 
readLineSerial2(400); 
        Serial2.write("reboot\r");
readLineSerial2(400); 
readLineSerial2(400); 

   Serial2.begin(115200, 256, 256);
    Serial.write(".");
 
 Serial.print("Init done!\n");
}

void loop()
{
  // 10hz
  if (millis()-fast_loopTimer > 99) {
    fast_loopTimer      = millis();
    
    sendwebsocketdata();
    
 //   String sendme = floattostring(roll) + ","+floattostring(pitch) + ","+floattostring(yaw);
    
  //  Serial.print(sendme);
   
  //     if (socketopen) {
//      httpsend();
//      socketopen = false;
//    }
  }
      
    xbee.update();
  
  // from serial to xbee
  while (Serial.available() > 0) {
    inByte = Serial.read();
    Serial1.write(inByte);
  }
  
  // from wifly to serial
    while (socketopen == false && Serial2.available() > 0) {
//      inByte = Serial2.read();
//      Serial.write(inByte);
    inByte = Serial2_read();
    }
    
             if (socketopen)   {
            httpsend();
//            while (socketopen) {
              xbee.update();
  //          }
          socketopen = false;
             }
    

}

void sendwebsocketdata() {
  
                         if (websocketrunning)     {
                            
                            
                            String sendme = floattostring(roll) + ","+floattostring(pitch) + ","+floattostring(yaw);
                            
                            Serial2.write(0x81);
                            Serial2.write((byte)sendme.length());
                            Serial2.print(sendme);
                                
//  Serial.printf_P(PSTR("pstr %1.2f,%1.2f,%1.2f\n"),roll ,pitch,yaw);
                         }
}

String floattostring(float f) 
{
  char output[20];
  dtostrf(f,0,2,output);
  return String(output);
}

int Serial2_read()
{
  int Byte = Serial2.read();
//  Serial.write(Byte);
restart:
    switch (requeststep) {
        case 0:
            if (Byte == '*') {
                requeststep++;
            } else {
                requeststep = 0;
            }
            break;
        case 1:
            if (Byte == 'O') {
                requeststep++;
            } else if (Byte == 'C') {
               websocketrunning = false;
               socketopen = false;
//               Serial.print("socket close ");
               requeststep = 0;
//               goto restart;
            }
           else {
                requeststep = 0;
                goto restart;
            }
            break;
        case 2:
            if (Byte == 'P') {
                requeststep++;
            } else {
                requeststep = 0;
            }
            break;
        case 3:
            if (Byte == 'E') {
                requeststep++;
            } else {
                requeststep = 0;
            }
            break;
        case 4:
            if (Byte == 'N') {
                requeststep++;
            } else {
                requeststep = 0;
            }
            break;
        case 5:
            if (Byte == '*') {
              
            requeststep = 0;
            
            socketopen = true;
//            Serial.print("socket open ");
            
            } else {
                    requeststep = 0;
            }
                                break;             
    }
    
    return Byte;
}

void httpsend()
{
httprestart: 
  int a = 0;
  
  Serial.println("httpsend()");
            
            String headline = String(readLineSerial2(10000));
            
            headline = headline.replace("*OPEN*","");
            headline = headline.replace("*CLOS*","");
            
            Serial.print("headline:");
            
            Serial.println(headline);
            
                        
            String file;
            int s1 = headline.indexOf(' ',0);
            int s2 = headline.indexOf(' ',s1+1);
            
            Serial.println("subs " + String(s1) + " " + String(s2));
            
            if (headline.startsWith("GET")) {
              // space + / = +2
              file = headline.substring(s1 + 2, s2);
            } else {
              // no post yet
              Serial.print("drop connection");
             return; 
            }
            
            
            String stringline = headline;
            String websocketkey = "";
            
            while (stringline > 3) {
              stringline = String(readLineSerial2(4000));
//              Serial.print("header: "+stringline);
              if (stringline.startsWith("Sec-WebSocket-Key:"))
              {
                websocketkey = stringline.substring(stringline.indexOf(':')+1,stringline.length());
                websocketkey = websocketkey.trim();
              }
            }

            
            if (file.startsWith("websocket")) {
              Serial.print("http websocket\n");
                            Serial2.println("HTTP/1.1 101 WebSocket Protocol Handshake");
                            Serial2.println("Upgrade: WebSocket");
                            Serial2.println("Connection: Upgrade");
                            Serial2.println("WebSocket-Location: ws://169.254.1.1:80/websocket/server");
                            Serial2.println("key: "+websocketkey);
                            
                            String accept = computewebsockethash(websocketkey);
                            
                            Serial2.println("Sec-WebSocket-Accept: " + accept);

                            Serial2.println("Server: Ardustation2");

                            Serial2.println("");
                            
                            websocketrunning = true;

            } else {
            websocketrunning = false;
              if (file == "") {
               file = "hud.htm"; 
              }
              
              Serial.print("http header\n");
                          
              char fn[99];
              file.toCharArray(fn,99);
              
              Serial.print("http file\n");
              
              writefile(fn);
              
//              goto httprestart;
            }
         
         Serial.print("http done\n");
                
}

char* readLineSerial2(int timeout) 
{
            char line[250];
            int a = 0;
            
            unsigned long t1 = millis();
            
            while (a < 249 && ((t1 + timeout) > millis())) 
            {
               if (Serial2.available()) {
                line[a++] = Serial2_read();
               }
        if (line[a-1] == '\n')
          break;
            };
            line[a++] = 0x0; 
        
            Serial.print("\nline: ");
            Serial.print(a-1,DEC);
            Serial.print(" ");
            Serial.print((char*)line);  
          
            return line;
}

String computewebsockethash(String key) 
{
  String magic = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
  String ret = key + magic;
  char temp[255];
  ret.toCharArray(temp,255);
  sha1_hash_t hash;
   sha1(&hash,temp,(ret.length() * 8));
   
   base64enc(temp,hash,sizeof(hash));
   
   Serial.println(ret);
   
   Serial.println((char*)temp);
   
   return String(temp);
}

void writefile(char* file) {
  // re-open the file for reading:
  
  Serial.println((char*)file);
  
  myFile = SD.open(file);
  if (myFile) {
    
      
            Serial2.write("HTTP/1.1 200 OK\r\n");
            if (String(file).endsWith("htm")) {
              Serial2.write("Content-Type: text/html\r\n");
            } else {
              Serial2.write("Content-Type: text/plain\r\n");
            }
            Serial2.write("Connection: close\r\n");
            Serial2.write("Pragma: no-cache\r\n");
            Serial2.write("Cache-Control: no-cache\r\n");
            Serial2.print("Content-Length: " + String(myFile.size()) + "\r\n");
            Serial2.write("\r\n");
    
    // read from the file until there's nothing else in it:
    while (myFile.available()) {
    	Serial2.write(myFile.read());
    }
    // close the file:
    myFile.close();
  } else {
    
      
            Serial2.write("HTTP/1.1 404 Not Found\r\n");
            Serial2.write("Content-Type: text/html\r\n");
         Serial2.write("Connection: close\r\n");
         Serial2.print("Content-Length: 31\r\n");
            Serial2.write("\r\n");
  	// if the file didn't open, print an error:
    Serial2.println("404 error opening file - dos fn");
  }
}

